#!/usr/bin/env python
# -*- encoding: utf-8 -*-
'''
@Description:       :步进电机控制类
@Date     :2023/05/30 09:37:33
@Author      :Cosecant
@version      :1.0
                    _ooOoo_                     
                   o8888888o                    
                  88   .   88                    
                   (| -_- |)                    
                   O\  =  /O                    
                ____/`---'\____                 
              .'   \|     |/   `.               
             /   \|||  :  |||/   \              
            /  _||||| -:- |||||_  \             
            |   | \ \  -  /// |   |             
            | \_|  ''\---/''  |_/ |             
            \  .-\__  `-`  ___/-. /             
          ___`. .'  /--.--\  `. . __            
       .'' '<  `.___\_<|>_/___.'  >' ''.         
      | | :  `- \`.;`\ _ /`;.`/ - ` : | |       
      \  \ `-.   \_ __\ /__ _/   .-` /  /       
 ======`-.____`-.___\_____/___.-`____.-'======   
                    `=---='                     
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^   
        佛祖保佑        永无BUG                  
'''
import serial
import time

DEBUG = True
status = (0x02, 0xE2, 0xEE)  #02:正常  E2:条件不满足  EE:错误


class StepperMotor:
    def __init__(self, myserial: serial.Serial, motor_id=0xe0):
        self.serial = myserial
        self.motor_id = motor_id
        self.command = bytes([])

    def __debug(self):
        if DEBUG:
            time.sleep(0.1)
            back = self.serial.read_all()
            print(back)
            if back[-1] == 0x6B:
                status_mapping = {
                    status[0]: '正常',
                    status[1]: '条件不满足',
                    status[2]: '错误'
                }
                print(status_mapping.get(back[-2], '未知状态'))
        else:
            return
    def __read_back(self):
        """
        @description  :读取返回值
        ---------
        @param  :None
        -------
        @Returns  :返回值 去掉帧头和帧尾的数据
        -------
        """
        start_time = time.time()
        frame_data = bytearray()
        frame_start = False
        error_flag = False
        while time.time() - start_time <= 1:
            if self.serial.in_waiting > 0:
                byte_data = self.serial.read()
                if byte_data == bytes([self.motor_id]):
                    frame_start = True
                elif frame_start == False:
                    if byte_data == bytes([0x00]):
                        continue
                    print('帧头错误')
                    return

                if byte_data != bytes([0x6B]):
                    frame_data += byte_data
                else:  #接收到帧尾
                    frame_data += byte_data
                    #判断是否报错
                    if DEBUG and frame_data[-2] == 0xEE:
                        print('错误')
                        return
                    data_frames=frame_data[2:-1]  #返回去掉帧头,命令号和帧尾的数据
                    print("get data:"+data_frames.hex())
                    return data_frames
        #超时
        print('超时')
        return None

    def cal_encoder(self):
        """
        @description  :命令功能：触发编码器校准
        """
        #命令格式：地址 + 0x06 + 0x45 + 校验字节
        self.command = bytes([self.motor_id, 0x06, 0x45, 0x6B])
        self.serial.write(self.command)
        self.__debug()

    def set_zero_position(self):
        """
        @description  :命令功能：设置当前位置为零点
        """
        # 命令格式：地址 + 0x0A + 0x6D + 校验字节
        self.command = bytes([self.motor_id, 0x0A, 0x6D, 0x6B])
        self.serial.write(self.command)
        self.__debug()

    def release_protection(self):
        """
        @description  :命令功能：解除保护
        """
        # 命令格式：地址 + 0x0E + 0x52 + 校验字节
        self.command = bytes([self.motor_id, 0x0E, 0x52, 0x6B])
        self.serial.write(self.command)
        self.__debug()

    def restore_factory_settings(self):
        """
        @description  :命令功能：恢复出厂设置
        """
        # 命令格式：地址 + 0x0F + 0x5F + 校验字节
        self.command = bytes([self.motor_id, 0x0F, 0x5F, 0x6B])
        self.serial.write(self.command)
        self.__debug()

    def read_firmware_and_hardware_version(self):
        """
        @description  :命令功能：读取固件版本号和硬件版本号
        -------
        @Returns  : str 固件版本+硬件版本号
        -------
        """
        # 命令格式：地址 + 0x1F + 校验字节
        self.command = bytes([self.motor_id, 0x1F, 0x6B])
        self.serial.write(self.command)
        #data[0]为固件版本，data[1]为硬件版本，例如：data[0] = 0x7D，data[1] = 0x6F，
        #固件版本 = 0x7D = ZDT_X57_V1.2.5，对应的硬件版本 = 0x6F = 111 = ZDT_X57_V1.1.1版本
        data = self.__read_back()
        if data:
            #解析data,从hex变为dec并转为str，数字之间用.隔开
            firmware_version = str(int(data[0]))  #固件版本
            firmware_version = '.'.join(firmware_version)
            hardware_version = str(int(data[1]))  #硬件版本
            hardware_version = '.'.join(hardware_version)
            return "固件版本: " + firmware_version + "  " + "硬件版本: " + hardware_version

    def read_phase_resistance_and_inductance(self):
        """
        @description  :命令功能：读取电机相电阻和电感
        -------
        @Returns  : int list 电机相电阻(mohm)和电感(uH)
        -------
        """
        # 命令格式：地址 + 0x20 + 校验字节
        self.command = bytes([self.motor_id, 0x20, 0x6B])
        self.serial.write(self.command)
        data = self.__read_back()
        #解析data data[0:2]为电机相电阻，data[2:]为电机电感
        if data:
            phase_resistance = int.from_bytes(data[0:2], byteorder='big')
            phase_inductance = int.from_bytes(data[2:], byteorder='big')
            return [phase_resistance, phase_inductance]

    def read_bus_voltage(self):
        """
        @description  :命令功能：读取母线电压
        -------
        @Returns  : int 总线电压(mV)
        -------
        """
        # 命令格式：地址 + 0x24 + 校验字节
        self.command = bytes([self.motor_id, 0x24, 0x6B])
        self.serial.write(self.command)
        data = self.__read_back()
        if data:
            bus_voltage = int.from_bytes(data[0:2], byteorder='big')
            return bus_voltage

    def read_bus_average_current(self):
        """
        @description  :命令功能：读取母线平均电流
        -------
        @Returns  : int 总线平均电流(mA)
        -------
        """
        # 命令格式：地址 + 0x26 + 校验字节
        self.command = bytes([self.motor_id, 0x26, 0x6B])
        self.serial.write(self.command)
        data = self.__read_back()
        if data:
            bus_average_current = int.from_bytes(data[0:2], byteorder='big')
            return bus_average_current

    def read_phase_current(self):
        """
        @description  :命令功能：读取电机相电流
        -------
        @Returns  : int 相电流(mA)
        -------
        """
        # 命令格式：地址 + 0x27 + 校验字节
        self.command = bytes([self.motor_id, 0x27, 0x6B])
        self.serial.write(self.command)
        data = self.__read_back()
        if data:
            phase_current = int.from_bytes(data[0:2], byteorder='big')
            return phase_current

    def read_encoder_raw_value(self):
        """
        @description  :命令功能：读取编码器原始值
        -------
        @Returns  : int 编码器原始值
        -------
        """
        # 命令格式：地址 + 0x30 + 校验字节
        self.command = bytes([self.motor_id, 0x30, 0x6B])
        self.serial.write(self.command)
        data = self.__read_back()
        if data:
            encoder_raw_value = int.from_bytes(data[0:2], byteorder='big')
            return encoder_raw_value

    def read_encoder_linear_value(self):
        """
        @description  :命令功能：读取经过线性化校准后的编码器值
        -------
        @Returns  : int 经过线性化校准后的编码器值
        -------
        """
        self.command = bytes([self.motor_id, 0x31, 0x6B])
        self.serial.write(self.command)
        data = self.__read_back()
        if data:
            encoder_linear_value = int.from_bytes(data[0:2], byteorder='big')
            return encoder_linear_value

    def read_motor_target_position(self):
        """
        @description  :命令功能：读取电机目标位置
        -------
        @Returns  : float 电机目标位置 单位：度 一位小数精度
        -------
        """
        self.command = bytes([self.motor_id, 0x33, 0x6B])
        self.serial.write(self.command)
        data = self.__read_back()
        if data:
            sign = int(data[0])
            motor_target_position = int.from_bytes(data[1:5], byteorder='big')
            motor_target_position = float(motor_target_position) / 10
            if sign == 1:
                motor_target_position = -motor_target_position
            return motor_target_position

    def read_motor_realtime_speed(self):
        """
        @description  :命令功能：读取电机实时速度
        -------
        @Returns  : float 电机实时速度 单位:(RPM 转/每分钟) 一位小数精度
        -------
        """
        self.command = bytes([self.motor_id, 0x35, 0x6B])
        self.serial.write(self.command)
        data = self.__read_back()
        if data:
            sign = int(data[0])
            motor_realtime_speed = int.from_bytes(data[1:3], byteorder='big')
            motor_realtime_speed = float(motor_realtime_speed) / 10
            if sign == 1:
                motor_realtime_speed = -motor_realtime_speed
            return motor_realtime_speed

    def read_motor_realtime_position(self):
        """
        @description  :命令功能：读取电机实时位置
        -------
        @Returns  : float 电机实时位置 单位：度 一位小数精度
        -------
        """
        self.command = bytes([self.motor_id, 0x36, 0x6B])
        self.serial.write(self.command)
        data = self.__read_back()
        if data:
            sign = int(data[0])
            motor_realtime_position = int.from_bytes(data[1:5],
                                                     byteorder='big')
            motor_realtime_position = float(motor_realtime_position) / 10
            if sign == 1:
                motor_realtime_position = -motor_realtime_position
            return motor_realtime_position

    def read_motor_position_error(self):
        """
        @description  :命令功能：读取电机位置误差
        -------
        @Returns  : float 电机位置误差 单位：度 两位小数精度
        -------
        """
        self.command = bytes([self.motor_id, 0x37, 0x6B])
        self.serial.write(self.command)
        data = self.__read_back()
        if data:
            sign = int(data[0])
            motor_position_error = int.from_bytes(data[1:5], byteorder='big')
            motor_position_error = float(motor_position_error) / 100
            if sign == 1:
                motor_position_error = -motor_position_error
            return motor_position_error

    def read_driver_realtime_temperature(self):
        """
        @description  :命令功能：读取驱动器实时温度
        -------
        @Returns  : int 驱动器实时温度 单位：摄氏度
        -------
        """
        self.command = bytes([self.motor_id, 0x39, 0x6B])
        self.serial.write(self.command)
        data = self.__read_back()
        if data:
            sign = int(data[0])
            driver_realtime_temperature = int.from_bytes(data[1:3],
                                                         byteorder='big')
            if sign == 1:
                driver_realtime_temperature = -driver_realtime_temperature
            return driver_realtime_temperature

    def motor_enable(self, enable=True,sycronize=False):
        """
        @description  :命令功能：电机使能
        -------
        @param  : enable=True 电机使能  enable=False 电机失能
        -------
        @Returns  : bool 电机使能状态
        -------
        """
        self.command = bytes([self.motor_id, 0xF3, 0xAB])
        self.command+=bytes([0x01]) if enable else bytes([0x00])
        self.command+=bytes([0x01]) if sycronize else bytes([0x00])
        self.serial.write(self.command)
        data = self.__read_back()
        if data:
            motor_enable_status = int(data[0])
            return True if motor_enable_status == 1 else False

    def torque_mode_control(self, slope: int,torque: int,synchronize=False):
        """
        @description  :命令功能：力矩模式控制
        -------
        @param  : torque 力矩 单位:mA  slope 电流斜率 单位:mA/s
        -------
        """
        self.command = bytes([self.motor_id, 0xF5])
        self.command += bytes([0x00]) if torque >= 0 else bytes([0x01])
        torque = abs(torque)
        slope = slope.to_bytes(2, 'big')
        torque = torque.to_bytes(2, 'big')
        self.command = self.command + slope + torque
        self.command += bytes([0x01, 0x6B]) if synchronize else bytes([0x00, 0x6B])
        self.serial.write(self.command)
        self.__debug()

    def speed_mode_control(self, speed: int, slope: int,synchronize=False):
        """
        @description  :命令功能：速度模式控制
        -------
        @param  : speed 速度*10 单位:RPM  slope 速度斜率*10 单位:RPM/s
        -------
        """
        self.command = bytes([self.motor_id, 0xF6])
        self.command += bytes([0x00]) if speed >= 0 else bytes([0x01])
        speed = abs(speed)
        slope = slope.to_bytes(2, 'big')
        speed = speed.to_bytes(2, 'big')
        self.command += slope + speed
        self.command += bytes([0x01, 0x6B]) if synchronize else bytes([0x00, 0x6B])
        self.serial.write(self.command)
        self.__debug()

    def position_mode_control_direct(self,
                                     position: int,
                                     relative_position=True,synchronize=False):
        """
        @description  :命令功能：直通位置模式控制
                        直通位置模式只需要方向 + 位置，它是直接以最大的加速度和速度让电机旋转到指定的目标位置，
                        可以通过相对位置/绝对位置标志来指明这条位置指令的位置是相对位置（相对当前位置）
                        还是绝对位置（上电/命令清0位置时为电机0位置）
        -------
        @param  : position 位置放大10倍如3600=360.0度 单位:度  relative_position=True 相对位置 relative_position=False 绝对位置
        -------
        """
        self.command = bytes([self.motor_id, 0xFA])
        self.command += bytes([0x00]) if position >= 0 else bytes([0x01])
        position = abs(position)
        position = position.to_bytes(4, 'big')
        self.command += position + bytes([0x00]) if relative_position else position + bytes([0x01])
        self.command += bytes([0x01, 0x6B]) if synchronize else bytes([0x00, 0x6B])
        self.serial.write(self.command)
        self.__debug()
    
    def position_mode_control_smooth(self,
                                     position: int,
                                     relative_position=True,
                                     max_speed=2000,
                                     synchronize=False):
        """
        @description  :命令功能：平滑位置模式控制
                        平滑位置模式可以依靠带宽值来调整电机运动的平滑性，
                        带宽越高，响应越快，电机运行越不平滑，反之则响应越慢，时间越长，越平滑，
                        尽量不要设置太低的带宽值，不然运行时间长。
                        可以通过相对位置/绝对位置标志来指明这条位置指令的位置是相对位置（相对当前位置）还是绝对位置（上电/命令清0位置时为电机0位置）。
        -------
        @param  : position 位置放大10倍如3600=360.0度 单位:度  relative_position=True 相对位置 relative_position=False 绝对位置
                    max_speed 最大速度*10 单位:RPM/s
        -------
        """
        self.command = bytes([self.motor_id, 0xFB])
        self.command += bytes([0x00]) if position >= 0 else bytes([0x01])
        position = abs(position)
        position = position.to_bytes(4, 'big')
        max_speed = max_speed.to_bytes(2, 'big')
        self.command += max_speed
        self.command += position + bytes([0x00]) if relative_position else position + bytes([0x01])
        self.command += bytes([0x01, 0x6B]) if synchronize else bytes([0x00, 0x6B])
        self.serial.write(self.command)
        self.__debug()

    def position_mode_control_trapezoid(self,
                                        position: int,
                                        relative_position=True,
                                        max_speed=2000,
                                        acceleration=1000,
                                        deceleration=1000,
                                        synchronize=False):
        """
        @description  :命令功能：梯形曲线位置模式控制
                        梯形曲线位置模式可以让电机按照“梯形”的形式进行加减速运动，可以让电机运行的比较平滑。
                        可以通过相对位置/绝对位置标志来指明这条位置指令的位置是相对位置（相对当前位置）还是绝对位置（上电/命令清0位置时为电机0位置）。
        -------
        @param  : position 位置放大10倍如3600=360.0度 单位:度  relative_position=True 相对位置 relative_position=False 绝对位置
                    max_speed 最大速度*10 单位:RPM/s  acceleration 加速度*10 单位:RPM/s^2  deceleration 减速度*10 单位:RPM/s^2
        -------
        """
        self.command = bytes([self.motor_id, 0xFD])
        self.command += bytes([0x00]) if position >= 0 else bytes([0x01])
        position = abs(position)
        position = position.to_bytes(4, 'big')
        max_speed = max_speed.to_bytes(2, 'big')
        acceleration = acceleration.to_bytes(2, 'big')
        deceleration = deceleration.to_bytes(2, 'big')
        self.command += acceleration + deceleration + max_speed
        self.command += position + bytes([0x00]) if relative_position else position + bytes([0x01])
        self.command += bytes([0x01, 0x6B]) if synchronize else bytes([0x00, 0x6B])
        self.serial.write(self.command)
        self.__debug()

    def stop(self):
        """
        @description  :命令功能：让电机以最大的速度和加速度立即停止转动。
        """
        self.command = bytes([self.motor_id, 0xFE, 0x98, 0x6B])
        self.serial.write(self.command)
        self.__debug()